#include "AS5600.h"
#include "Wire.h"
#include <Arduino.h>


#define _2PI 6.28318530718f


//解算编码器返回目前的角度数据，处于[0,6.8]之间
double Sensor_AS5600::getSensorAngle()
{
    uint8_t angle_reg_msb = 0x0C;

    byte readArray[2];
    uint16_t readValue = 0;

    wire->beginTransmission(0x36);
    wire->write(angle_reg_msb);
    wire->endTransmission(false);

    wire->requestFrom(0x36, (uint8_t)2);
    for (byte i = 0; i < 2; i++)
    {
        readArray[i] = wire->read();
    }

    int _bit_resolution = 12;
    int _bits_used_msb = 11 - 7;
    float cpr = pow(2, _bit_resolution);
    int lsb_used = _bit_resolution - _bits_used_msb;

    uint8_t lsb_mask = (uint8_t)((2 << lsb_used) - 1);
    uint8_t msb_mask = (uint8_t)((2 << _bits_used_msb) - 1);

    readValue = (readArray[1] & lsb_mask);
    readValue += ((readArray[0] & msb_mask) << lsb_used);
    return (readValue / (float)cpr) * _2PI;
}

Sensor_AS5600::Sensor_AS5600(int Mot_Num)
{
    _Mot_Num = Mot_Num;
}

void Sensor_AS5600::Sensor_init(twoWire *_wire)
{
    // enable the wire 
    wire = _wire;
    wire->begin(); // 电机Sensor
    delay(500);

    // get the Sensor Angle
    getSensorAngle();
    delayMicroseconds(1);
    vel_angle_prev = getSensorAngle();
    vel_angle_prev_ts = micros();

    delay(1);
    getSensorAngle();
    delayMicroseconds(1);
    angle_prev = getSensorAngle();
    angle_prev_ts = micros();

    return;
}

void Sensor_AS5600::Sensor_update()
{
    float val = getSensorAngle();
    angle_pre_ts = micros();
    float d_angle = val - angle_prev;

    if (abs(d_angle) > (0.8f * _2PI))
        full_rotations += (d_angle > 0) ? -1 : 1;
    angle_prev = val;
}

float Sensor_AS5600::getMechanicalAngle()
{
    return angle_prev;
}

float Sensor_AS5600::getAngle()
{
    return (float)full_rotations * _2PI + angle_prev;
}

float Sensor_AS5600::getVelocity()
{
    float Ts = (angle_prev_ts - vel_angle_prev_ts) * 1e-6;

    //修复微溢出
    if(Ts <= 0) Ts = 1e-3f;

    float vel = ((float)(full_rotations - vel_full_rotations) * _2PI + (angle_prev - vel_angle_prev)) / Ts;

    vel_angle_prev = angle_prev;
    vel_full_rotations = full_rotations;
    vel_angle_prev_ts = angle_prev_ts;

    return vel;
}